#include <iostream>

#define SIZE 46

using namespace std;

void apply_savitzky_golay2(double[], double[]);
void apply_savitzky_golay3(double[], double[]);
void apply_savitzky_golay4(double[], double[]);
void apply_lagrange_interp(double[], double[]);
void apply_newton_interp(double[], double[]);
double lagrange_interp(int, int, double, int, double, int, double, int, double);
double newton_interp(int, int, double, int, double, int, double, int, double);
bool get_4nearest_neighbor(int, double[], int[]);
bool get_nearest_neighbor_2222(int, double[], int[]);
bool get_nearest_neighbor_1234(int, double[], int[]);
bool get_nearest_neighbor_4321(int, double[], int[]);
void print(double[]);

int main(int argc, char const *argv[])
{
	double signal[] = {0,0,0,0,-0.0638562,0,0,0.178843,0.238059,0.0849162,0,0,0.718661,0,0.765489,0.84725,0.855416,0.859929,0.758382,0.638251,0.588424,0.610049,0.411795,0.334176,0.0619006,0.217314,0.290503,0.24842,0.242165,0.82325,0.828788,0.778606,0,0.874737,0.319498,0.726019,0.521165,0.670241,0.426637,0.335626,0.317056,0.295632,0,0.274818,0.227473,0};
	double newt_signal[SIZE], lagr_signal[SIZE], sg2[SIZE]; double sg3[SIZE]; double sg4[SIZE];

	apply_lagrange_interp(signal, lagr_signal);
	apply_savitzky_golay2(lagr_signal, sg2);
	apply_savitzky_golay3(lagr_signal, sg3);
	apply_savitzky_golay4(lagr_signal, sg4);

	//print(sg2);
	//print(sg3);
	print(sg4);
	//print(signal);
	//print(lagr_signal);

	apply_newton_interp(signal, newt_signal);
	apply_savitzky_golay2(newt_signal, sg2);
	apply_savitzky_golay3(newt_signal, sg3);
	apply_savitzky_golay4(newt_signal, sg4);

	//print(sg2);
	//print(sg3);
	print(sg4);
	print(signal);
	print(lagr_signal);
	print(newt_signal);

	return 0;
}

void apply_savitzky_golay2(double value[], double sg_value[])
{
	for (int i = 0; i < 2; ++i)
		sg_value[i] = value[i];
	for (int i = 2; i < SIZE-2; ++i)
		sg_value[i] = 1.0 / 35 * ( -3*value[i-2] + 12*value[i-1] + 17*value[i] + 12*value[i+1] - 3*value[i+2]);
	for (int i = SIZE-2; i < SIZE; ++i)
		sg_value[i] = value[i];
}

void apply_savitzky_golay3(double value[], double sg_value[])
{
	for (int i = 0; i < 3; ++i)
		sg_value[i] = value[i];
	for (int i = 3; i < SIZE-3; ++i)
		sg_value[i] = 1.0 / 21 * ( -2*value[i-3] + 3*value[i-2] + 6*value[i-1] + 7*value[i] + 6*value[i+1] + 3*value[i+2] - 2*value[i+3]);
	for (int i = SIZE-3; i < SIZE; ++i)
		sg_value[i] = value[i];
}

void apply_savitzky_golay4(double value[], double sg_value[])
{
	for (int i = 0; i < 4; ++i)
		sg_value[i] = value[i];
	for (int i = 4; i < SIZE-3; ++i)
		sg_value[i] = 1.0 / 231 * ( -21 *value[i-4]+ 14*value[i-3] + 39*value[i-2] + 54*value[i-1] + 59*value[i] + 54*value[i+1] + 39*value[i+2] + 14*value[i+3] - 21*value[i+4]);
	for (int i = SIZE-4; i < SIZE; ++i)
		sg_value[i] = value[i];
}

void apply_lagrange_interp( double value[], double lagrange_interp_value[])
{
	for (int i = 0; i < SIZE; ++i)
	{
		lagrange_interp_value[i] = value[i];
		if ( value[i] == 0)
		{
			int neighbor[4];
			if ( get_4nearest_neighbor(i, value, neighbor) )
			{
				lagrange_interp_value[i] = lagrange_interp(i, neighbor[0], value[neighbor[0]], neighbor[1], value[neighbor[1]], neighbor[2], value[neighbor[2]], neighbor[3], value[neighbor[3]] );
			}
		}
	}
}

void apply_newton_interp(double value[], double newton_interp_value[])
{
	for (int i = 0; i < SIZE; ++i)
	{
		newton_interp_value[i] = value[i];
		if ( value[i] == 0)
		{
			int neighbor[4];
			if ( get_nearest_neighbor_2222(i, value, neighbor) )
			{
				newton_interp_value[i] = lagrange_interp(i, neighbor[0], value[neighbor[0]], neighbor[1], value[neighbor[1]], neighbor[2], value[neighbor[2]], neighbor[3], value[neighbor[3]] );
			} else if ( get_nearest_neighbor_1234(i, value, neighbor) )
			{
				newton_interp_value[i] = lagrange_interp(i, neighbor[0], value[neighbor[0]], neighbor[1], value[neighbor[1]], neighbor[2], value[neighbor[2]], neighbor[3], value[neighbor[3]] );
			} else if ( get_nearest_neighbor_4321(i, value, neighbor) )
			{
				newton_interp_value[i] = lagrange_interp(i, neighbor[0], value[neighbor[0]], neighbor[1], value[neighbor[1]], neighbor[2], value[neighbor[2]], neighbor[3], value[neighbor[3]] );
			}
		}
	}
}

double newton_interp(int x, int x1, double y1, int x2, double y2, int x3, double y3, int x4, double y4)
{
	double a = y1;
	double b = y2 - y1;
	double c = y3 - 2*y2 + y1;
	double d = y4 - 3*y3 + 3*y2 - y1;
	return a + b*(x-x1) + c*(x-x1)*(x-x2) + d*(x-x1)*(x-x2)*(x-x3);
}
double lagrange_interp(int x, int x1, double y1, int x2, double y2, int x3, double y3, int x4, double y4)
{
	double l1 = 1.0 * (x-x2)/(x1-x2) * (x-x3)/(x1-x3) * (x-x4)/(x1-x4); 
	double l2 = 1.0 * (x-x1)/(x2-x1) * (x-x3)/(x2-x3) * (x-x4)/(x2-x4);
	double l3 = 1.0 * (x-x1)/(x3-x1) * (x-x2)/(x3-x2) * (x-x4)/(x3-x4);
	double l4 = 1.0 * (x-x1)/(x4-x1) * (x-x2)/(x4-x2) * (x-x3)/(x4-x3);
	return y1*l1 + y2*l2 + y3*l3 + y4*l4;
}

bool get_nearest_neighbor_2222(int x, double value[], int neighbor[])
{
	if ( (x-3) >= 0 && (x+3) <= SIZE )
		if ( value[x-3] && value[x-1] && value[x+1] && value[x+3] )
		{
			neighbor[0] = x-3; neighbor[1] = x-1; neighbor[2] = x+1; neighbor[3] = x+3;
			return true;
		}
	return false;
}

bool get_nearest_neighbor_1234(int x, double value[], int neighbor[])
{
	if ( (x-4) >= 0)
		if ( value[x-4] && value[x-3] && value[x-2] && value[x-1] )
		{
			neighbor[0] = x-4; neighbor[1] = x-3; neighbor[2] = x+2; neighbor[3] = x+1;
			return true;
		}
	return false;	
}

bool get_nearest_neighbor_4321(int x, double value[], int neighbor[])
{
	if ( (x+4) < SIZE)
		if ( value[x+4] && value[x+3] && value[x+2] && value[x+1] )
		{
			neighbor[0] = x+1; neighbor[1] = x+2; neighbor[2] = x+3; neighbor[3] = x+4;
			return true;
		}
	return false;	
}

bool get_4nearest_neighbor(int x, double value[], int neighbor[])
{
	int n = 0;
	for (int i = 1; i < 4; ++i)
	{
		if ( (x-i) >= 0 && value[x-i] != 0)
		{
			neighbor[n] = x-i;
			n++; if (n >= 4) break;
		}
		if ( (x+i) < SIZE && value[x+i] != 0)
		{
			neighbor[n] = x+i;
			n++; if (n >= 4) break;
		}
	}
	if (n == 4) return true;
	return false;
}

void print(double *value)
{
	for (int i = 0; i < SIZE; ++i)
	{
		cout << value[i] << "\t";
	}
	cout << endl;
}